#include "robot_one_navigation/nav_controller_test.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_util/geometry_utils.hpp"

namespace nav_controller_test
{
    void NavControllerTest::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        node_ = parent.lock();
        name_ = name;
        buffer_ = tf;
        costmap_ros_ = costmap_ros;

        nav2_util::declare_parameter_if_not_declared(node_, name_ + ".max_linear_speed", rclcpp::ParameterValue(0.1));
        node_->get_parameter(name_ + ".max_linear_speed", max_linear_speed_);
        nav2_util::declare_parameter_if_not_declared(node_, name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
        node_->get_parameter(name_ + ".max_angular_speed", max_angular_speed_);
    }

    void NavControllerTest::cleanup()
    {
        RCLCPP_INFO(node_->get_logger(), "清理控制器：%s,类型为nav_controller_test::NavControllerTest", name_.c_str());
    }

    void NavControllerTest::activate()
    {
        RCLCPP_INFO(node_->get_logger(), "激活控制器：%s,类型为nav_controller_test::NavControllerTest", name_.c_str());
    }

    void NavControllerTest::deactivate()
    {
        RCLCPP_INFO(node_->get_logger(), "反激活控制器：%s,类型为nav_controller_test::NavControllerTest", name_.c_str());
    }

    void NavControllerTest::setPlan(const nav_msgs::msg::Path &path)
    {
        path_ = path;
    }

    geometry_msgs::msg::TwistStamped NavControllerTest::computeVelocityCommands(
        const geometry_msgs::msg::PoseStamped &pose,
        const geometry_msgs::msg::Twist &,
        nav2_core::GoalChecker *)
    {
        // 检查路径是否为空
        if (path_.poses.empty())
        {
            throw nav2_core::PlannerException("收到长度为0的路径！");
        }
        // 机器人位姿转换到全局坐标系
        geometry_msgs::msg::PoseStamped global_pose;
        if (!nav2_util::transformPoseInTargetFrame(pose, global_pose, *buffer_, path_.header.frame_id, 0.1))
        {
            throw nav2_core::PlannerException("无法将机器人位姿转换为全局计划的坐标系中！");
        }

        auto target_pose = getNearTargetPose(global_pose);
        auto diff_angle = calculateAngleDifference(global_pose, target_pose);

        geometry_msgs::msg::TwistStamped cmd_vel;
        cmd_vel.header.frame_id = global_pose.header.frame_id;
        cmd_vel.header.stamp = node_->now();
        if (fabs(diff_angle) > M_PI / 10)
        {
            cmd_vel.twist.linear.x = 0.;
            cmd_vel.twist.angular.z = fabs(diff_angle) / diff_angle * max_angular_speed_;
        }
        else
        {
            cmd_vel.twist.linear.x = max_linear_speed_;
            cmd_vel.twist.angular.z = 0;
        }
        RCLCPP_INFO(node_->get_logger(), "控制器%s发送速度（%f,%f）", name_.c_str(), cmd_vel.twist.linear.x, cmd_vel.twist.angular.z);
        return cmd_vel;
    }

    void NavControllerTest::setSpeedLimit(const double &speed_limit, const bool &percentage)
    {
        (void)speed_limit;
        (void)percentage;
    }
    // 获取路径中距离当前点最近的点
    geometry_msgs::msg::PoseStamped NavControllerTest::getNearTargetPose(const geometry_msgs::msg::PoseStamped &current_pose)
    {
        using nav2_util::geometry_utils::euclidean_distance;
        int nearest_pose_index = 0;
        double min_distance = euclidean_distance(current_pose, path_.poses.at(0));
        for (size_t i = 1; i < path_.poses.size(); i++)
        {
            double distance = euclidean_distance(current_pose, path_.poses.at(i));
            if (distance < min_distance)
            {
                min_distance = distance;
                nearest_pose_index = i;
            }
        }
        path_.poses.erase(std::begin(path_.poses), std::begin(path_.poses) + nearest_pose_index);
        if (path_.poses.size() == 1)
        {
            return path_.poses.at(0);
        }
        return path_.poses.at(1);
    }
    // 计算目标点方向和当前朝向的角度差
    double NavControllerTest::calculateAngleDifference(const geometry_msgs::msg::PoseStamped &current_pose,
                                                       const geometry_msgs::msg::PoseStamped &target_pose)
    {
        double current_yaw = tf2::getYaw(current_pose.pose.orientation);
        double target_angle = std::atan2(target_pose.pose.position.y - current_pose.pose.position.y,
                                         target_pose.pose.position.x - current_pose.pose.position.x);
        double diff_angle = target_angle - current_yaw;
        if (diff_angle < -M_PI)
        {
            diff_angle += 2 * M_PI;
        }
        else if (diff_angle > M_PI)
        {
            diff_angle -= 2 * M_PI;
        }
        return diff_angle;
    }

} // namespace nav_controller_test

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav_controller_test::NavControllerTest, nav2_core::Controller)